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一 种 面向 机 器 人 机 械 系 统 的 程式 化 动力 学 建 模 方 法 
庄 未 , 康文 杰 , 黄 用 华 , 高 振 宇 , 杨 继 伟 


(桂林 电子 科技 大 学 机 电工 程 学 院 , 广 西 桂林 541004) 


摘 要 :针对 机 器 人 动力 学 模型 复杂 、 宛 余 计算 导致 运算 效率 低 的 问题 ,提出 一 种 程式 化 建 模 方法 (PMM)。 以 六 自由 度 斯 
坦 福 机 械 臂 为 例 ,利用 该 方法 建立 基于 拉 格 朗 日 方程 的 动力 学 模型 ,按照 “ 正 向 分 析 ,逆序 输出 ?的 核心 思想 ,重点 分 析 模 型 
递 推 过 程 ;在 验证 模型 正确 性 的 基础 上 , 比较 使 用 PMM 和 未 使 用 PMM 的 常规 拉 格 朗 日 方程 所 建立 的 斯 坦 福 机 械 辟 力学 
模型 在 计算 机 中 的 “尺寸 ?和 运行 时 间 等 指标 。 实 验 结果 表明 ,相对 常规 拉 格 朗 日 方法 ,由 了 PMM 所 建 模型 的 复杂 程度 降低 
了 67.6% ,计算 效率 提高 了 66.3%; 斯 坦 福 机 械 辟 为 完整 约束 系统 ,将 PMM 推广 到 欠 驱 动 非 完 整 约束 系统 ,采用 与 模型 紧 
密 相 关 的 部 分 反馈 线性 化 控制 算法 进行 数值 仿真 和 物理 样机 实验 分 析 , 验 证 了 PMM 的 可 靠 性 和 有 效 性 ,为 不 同类 型 的 机 
知人 提供 了 一 种 效率 高 .通用 性 强 的 动力 学 建 模 方法 。 
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A programmed dynamic modeling method for robot mechanical System 
ZHUANG Wei, KANG Wenjie, HUANG Yonghua，GAO Zhenyu, YANG Jiwei 


(School of Mechanical and Electrical Engineering, Guilin University of Electronic Technology, Guilin 541004, China) 


bstract: Aiming at the problem of low efficiency caused by complex and redundant calculation of robot dynamics model, a 
_ 人 Fbgrammed modeling method is proposed. Taking the Stanford Arm with six degrees of freedom as an example, the dy- 
mic model based on Lagrangian equation is established by using this method. According to the core idea of "forward analy- 
Ist$, reverse output", the recursive process of the model is analyzed emphatically. On the basis of verifying the correctness of 
he model, the indexes such as the dimensions and running time of the Stanford Arm model based on the PMM and the con- 
-Sentional Lagrange equation without the use of the PMM are compared. The results show that relative to the conventional 
Morange method, the complexity of the model established by PMM is reduced by 67.6%, and the computational efficiency 
is increased by 66. 3%. Stanford Arm is a complete constrained system. PMM is extended to underactuated nonholonomic 


constrained systems, numerical simulation and physical prototype experiment analysis are carried out by using partial feed- 


back linearization control algorithm which is closely related to the model, it’s reliability and effectiveness of the programmed 
modeling method are verified，which provides a modeling method with higher efficiency and better versatility for different 
types of robots. 
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复杂 系统 动力 学 模型 是 进行 运动 控制 的 重要 基 ”一 种 方法 为 基于 分 析 系 统 力 和 力矩 矢量 的 建 模 方法 ， 
础 ,而 动力 学 建 模 方法 在 很 大 程度 上 决定 了 模型 的 准 。 ”其 属于 牛顿 经 典 力学 范畴 ,例如 牛顿 - 欧 拉 (NE) 法 和 
确 性 和 计算 效率 ,从 而 影响 控制 效果 ,因此 受到 了 许 Kane 方法 。Kvrgic 等 中 认为 NE 法 是 推导 机 械 臂 动 
多 学 者 的 广泛 关注 。 力学 模型 的 一 种 非常 有 效 的 计算 方法 , 且 将 该 方法 应 
近年 来 ,机 器 人 动力 学 建 模 方 法 主要 有 两 种 。 第 ”用 于 六 自由 度 机 器 人 ,并 与 其 他 方法 的 计算 效率 进行 
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比较 。 王 健 等 中 根据 NE 法 建立 了 四 旋 骂 飞行 器 的 
非 线性 模型 并 设计 控制 器 ,实现 了 对 四 旋翼 飞行 髓 系 
统 双环 控制 。 赵 丽 美 中 和 Ghariblu 等 所 采用 NE 法 
分 别 建立 蛇 形 机 器 人 和 新 型 移动 球形 机 器 人 动力 学 
模型 , 均 实现 了 基于 模型 的 运动 控制 研究 ,特别 是 后 
者 提出 的 动力 学 方程 中 有 关 微 分 代数 方程 的 计算 方 
法 。Luh 等 趾 开 发 出 一 种 效率 更 高 的 递归 NE 法 , 独 
立 于 机 器 人 配置 的 类 型 ,文献 L6-8] 中 有 着 许多 类 似 
的 应 用 。Kane 方法 综合 分 析 力 学 和 矢量 力学 的 优 
点 ,以 "广义 速率 ”取代 “广义 坐标 ?作为 独立 变量 , 引 


之 里 ， 


运动 学 的 正 向 分 析 和 系统 动力 学 模型 的 逆序 输出 , 解 
决 了 建 模 过 程 中 模型 复杂 、 输 出 膨胀 等 问题 。 


1 方法 描述 


传统 建 模 方 法 从 系统 广义 坐标 逐步 计算 动力 学 
方程 所 需 元 素 , 然 而 建 模 过 程 实则 是 符号 计算 的 过 
程 ,这 将 使 变量 表达 式 被 层 层 迭 代 , 容易 导致 模型 输 
出 膨胀 。PMM 解决 了 这 一 问题 ,其 大 体 分 为 系统 运 
动 学 的 正 向 分 析 和 系统 动力 学 模型 的 逆序 输出 2 个 
步骤 。 正 向 分 析 是 对 所 研究 对 象 的 系统 分 析 ,包括 坐 


入 偏 速度 、 偏 角速度 独特 概念 ,着 手 于 广义 主动 力 、J 

义 惯 性 力 的 计算 。Hnussain 等 中 认为 Kane 方法 优 于 
NE 法 和 Lagrange 方 法 ,并 利用 Kane 方法 建立 了 三 
自 南 度 机构 运 动 链 的 动力 学 模型 ,可 为 研究 Kane 方 
涛 的 初学 者 提供 了 指导 性 的 意见 。Huang 等 "采用 
Kh 方程 以 螺旋 理论 的 形式 建立 了 前 轮 驱 动 自行 车 
上 句 人 的 动力 学 模型 ， 为 其 他 类 似 非 完整 约束 系统 建 
模 包 供 了 参考 。Kane 等 "建立 了 基于 Kane 方程 的 
斯 坦 福 机 械 臂 的 动力 学 模型 ,与 NE 法 建立 


3 
的 模型 在 运算 次 数 上 作 比较 ,结果 表明 Kane 方法 计 
入 率 更 高 。 庄 未 等 "2 采用 同样 的 方法 建立 了 球形 
桃 器 人 的 动力 学 模型 ,并 完成 了 直线 和 S 曲线 运动 的 
侨 加 分析。 

_ 伍 第 二 种 是 基于 分 析 系 统 能 量 和 功 的 建 模 方法 , 即 
属 接 分 析 力 学 范畴 ， 如 基于 Chaplygin 方程 或 工 - 
agnge 方程 的 建 模 方法 。Zhang 等 5 采用 二 
ge 方程 建立 球形 机 器 人 动力 学 模型， 实现 了 对 整 

的 实时 控制 。Ruan 等 5 利用 yl 
着 泽 对 独 轮 车 机 器 人 进行 建 模 ,并 完成 了 一 系列 的 实 
验 。 李 梦 飞 [5 基于 Lagrange 方程 建立 了 六 自由 度 
机 械 辟 的 动力 学 模型 ,并 通过 ADAMS 动力 学 仿真 
验证 了 模型 的 正确 性 。 文 献 L16-19 给 出 了 Lagrange 
方程 在 其 他 机 械 辟 及 多 自由 度 机 器 人 上 的 应 用 。 
于 欠 驱 动 非 完整 约束 系统 ,在 文献 [20-21] 中 应 
L Woe 方程 的 另 一 种 形式 Chaplygin 方程 or 

了 变 结构 两 轮 车 机 器 人 和 独 轮 车 机 器 人 的 动力 学 
Te 系列 仿真 分 析 和 物理 样机 实验 。 

通过 对 上 述 文献 中 关于 动力 学 建 模 方法 的 研究 
可 知 , 牛 顿 经 典 力学 分 析 过 程 繁 杂 , 会 产生 大 量 的 约 
东 反 力 ,对 于 复杂 的 力学 系统 ,其 难以 直接 应 用 ;基于 
能 量 分 析 的 方法 , 随 着 自由 度 的 增加 ,其 模型 输出 容 
易 产 生 横向 膨胀 ,导致 偏 导 数 、. 导数 计算 难以 进行 , 即 
所 谓 的 维 数 灾难 , 带 来 重复 计算 、 运 行 效率 低下 的 
问题 。 
鉴于 此 ,提出 一 


种 程式 化 建 模 方法 ,通过 对 系统 


人 角速度 和 质心 速度 分 析 、 运 动 约束 分 析 、 动 
能 和 力 函 数 分 析 、 变 量 草 含 关系 分 析 等 ;逆序 输出 是 
虽 将 动力 学 方程 所 需 元 素 的 计算 结果 按 逆序 输出 , 确 

保 模型 不 膨胀 。 图 1 为 PMM 总 体 思路 示意 。 


笃 标 系 、 广 又 亿 和 标 分 析 羡 做 傈 条 兴 信 因 关 得 
速度 、 质 心 速度 分 析 贱 。 。 站 
动能 和 力 函 数 分 析 ” 蜀 动力 学 方程 


步骤 2 


Order 
设置 输出 深度 NN 

YW 
VARNAMEEA 

ay 通 序 要 到 变 最 和 表达 式 ， 
ER A Take[VARNAME,-(NN-Order)] 
ESS oe | rake[EXPRESSION,-(NN-Order) 
Vv 
= 输出 模型 到 指定 路 彻 
名 和 表达 式 : 


ee 
EXPRESSION[[Order]]=... 


结束 


图 1 PMM 总 体 思路 


PMM 的 核心 思想 : 正 向 分 析 开 始 于 广义 坐标 ， 
中 间 包 括 速度 、 非 完整 约束 分 析 等 ,终止 于 系统 的 动 
能 和 势能 ,其 关键 在 于 引入 中 间 变 量 后 建立 变量 的 隶 
属 关系 表 ， 逆序 输出 始 于 动能 (热能) 对 广义 坐标 或 广 
义 速度 的 偏 导数 ,通过 变量 隶属 关系 ,根据 链 式 求 导 


的 方法 逐步 传播 扩散 ,最 终 分 析出 最 原始 、 最 基本 的 
偏 导数 项 , 并 按 链 式 求 导 时 先后 顺序 的 逆序 进行 
输出 。 


在 实际 建 模 过 程 中 ,以 上 技术 手段 尚 不 能 完全 保 
证 输出 最 合理 ,最 简洁 的 力学 模型 ,有 时 会 遇 到 0 变 


量 、 变 量 重复 定义 和 顺序 错乱 等 问题 ,图 2 所 示 算 法 
用 以 解决 上 述 问题 。 
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始 始 始 
MU 
变量 名 和 表达 式 分 别 赋值 给 变量 名 和 表达 式 分 别 赋 给 变量 名 和 表达 式 分 别 赋 给 
VARNAME 和 EXPRESSION VARNAME 和 EXPRESSION VARNAME 和 EXPRESSION 
v 
将 EXPRESSION 中 与 0 相 掖 顺 序 比较 VARNAME 每 一 个 元 有 1 
[中 同 的 元 素 位 置 赋 给 TEMP 秋思 YVARNAME 列 居 ， 记录 下 条 
同 元 素 位 置 并 赋值 给 TEMP 
二 < | 


TEMP 长 度 大 于 


站 
TEMP 中 立 置 为 除 
针 的 变量 名 赋 信 为 0 竹 YOR 和 和 
元 素 的 变量 名 赋值 为 0 
0 变量 被 代入 


EXPRESSION 并 计算 记录 VARNAME 中 0 变 
量 位 置 并 赋 给 TEMP1 
删除 VARNAME 和 

二 -| EXPRESSION 中 位 置 为 删除 VARNAME 和 EXPRESSION 
了 邓 ”| TEMP 中 所 有 元 素 的 位 置 中 位 置 为 TEMP1 中 所 有 元 素 的 元 素 
G) 
©O 输出 VARNAME 输出 VARNAME 
© 输出 EXPRESSION 输出 EXPRESSION 
© 
© 结束 
©O 
™ 
CN 
AN 
© 
CN 
.之 


X 


china 


听 坦 福 机 械 臂 建 模 与 仿真 分 析 


为 更 加 直观 地 介绍 PMM 和 说 明 其 高 效 性 ,应 用 
该 方法 建立 斯 坦 福 机 械 臂 动力 学 模型 2 ,机 构 简 图 
如 图 3 所 示 。 

它 由 6 个 构件 组 成 ,分 别 记 为 B; (i 二 1~6),B， 
通过 滑动 副 与 B, 相连 ,以 实现 平移 运动 ;B,、B,、B。 
通过 相应 旋转 关节 分 别 与 B1 、.B;、Bs 相连 , 作 定 轴 
转动 。 

坐标 系 及 广义 坐标 描述 : 

1) ,i1\、j 二 1~3, 表 示 B; 坐标 系 Xx、y、z 轴 ; 

e ”为 大 地 坐标 系 ,e* ”始终 与 地 面 垂 直 ; 

0 
致 ,坐标 系 ”和 e” 原点 重合 

4) 选 取 wj， 王 1 一 6, 表 示 系 统 的 6 个 广义 坐标 。 

建 模 过 程 中 所 使 用 的 符号 及 含义 如 表 1 所 示 。 
特别 说 明 :& 、m、n 均 属于 i。 


一 > 


有 ANNE v 四 
前 的 所 有 元 素 组 成 的 列表 ， 
录 相 同 元 素 的 位 置 并 赋 给 TEMP 


到 


将 该 元 素 和 与 之 对 应 的 表达 式 分 别 插入 
位 置 为 TEMP 中 元 素 的 VARNAME 和 


EXPRESSION 中 ， 分 别 删除 VARNAME 
和 EXPRESSION 中 放下 后 一 位 置 的 变 


和 表达 式 


H<= 边 界 条 件 ? 
不 
4 [= 

输出 VARNAME 
输出 EXPRESSION 


结束 


图 2 剔除 0 变量 .剔除 重复 变量 及 变量 排序 算法 流程 


表 1 系统 变量 定义 
含义 


符号 
gi(gi) B,; 相对 也，， 转角 或 距离 (角速度 或 线 速度 ) 
oo 名 (名 ) B; 角速度 (质心 线 速 度 ) 在 e 必 下 的 矢量 描述 
6 B,; 质心 (e 中 原点 ) 位 置 在 e“’ 下 的 矢量 描述 
vo e 中 原点 线 速度 在 e。* 下 的 矢量 描述 
mR ce 系 到 ce 系 的 旋转 变换 矩阵 
g 重力 加 速度 
2.1 正 向 分 析 过 程 
根据 机 器 人 学 公式 可 知 ,e"” 转换 为 ec” 的 旋转 
变换 和 矩 阵 . 
R=Rot(y,—g;).* Rot(x,—g,)* Rot(y,— gi), 
(1) 
其 中 ,Rot(p, 一 gq;)(p 二 x,，y ;jj 二 1~~3) 为 绕 P 轴 


转 一 q; 的 旋转 变换 矩阵 。 
由 刚体 运动 学 原理 可 知 ,B, 角速度 
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oa 为 转动 动能 和 平 动 动能 , 则 有 
6 
e 0) 
| T= > Mi ve Vee TJ; * Op * WB! )/2, 
> 之 Bi Bi Bi Bi 
4 (4) 
PN | 其 中 :M; 与 所 为 三 维 质量 和 矩阵 , 且 MM; 二 mT,m; 为 
必 “| oo 1 为 3X3 单位 阵 ; J; = diag (J ,J ,， 
om 1 .) ,其 中 了 ,了 ,J 为 刚体 B; 绕 过 质心 对 应 的 转动 
加 质心 eo 
bs | wa ha 
0 | -| 以 en 原点 为 零 热 能 参考 面 ,B，; 高 度 , 则 
- | gs 
Le es hs 一 (CRR Ie 十 ?及 四 L152 HR. J (5) 
es | B, ea 注 :L24 表 示 取 矢量 第 二 个 元 素 , 即 矢量 在 e, 轴 上 的 
8 a Lo > 投影 (质心 高 度 )。 
EE 一 "re 一 7 
2 其 他 刚体 质心 高 度 计算 类 似 , 求 得 系统 总 重力 
势能 
6 
U= mh,.g, (6) 
1 一 1 
图 3 ”斯坦福 机 械 避 机 构 2.2 逆序 输出 过 
基于 拉 格 朗 日 方程 的 动力 学 模型 : 
一 及 “ie 十 RotCy， 一 03)。Ror(Cz， 一 q)。 加 aT | aU 
CN dzel Tdses, (2) dt \9g; 9g; 9g; 本 (7) 
1,e;(j 二 1~3) 为 第 j 个 轴 的 单位 矢量 。 其 中 ,zz 为 关节 驱动 力矩 。 
以 wb 为 参考 ,其 他 各 刚体 角速度 均 在 自身 坐 如 前 所 述 ,逆序 输出 过 程 始 于 动能 (势能 ) 对 广义 
Se 2 所 示 。 We 、 
_ 坐标 或 广义 速度 衣 ,将 式 
表 2 刚体 角速度 人 二 
(4) 化 为 系数 与 角速度 乘积 之 和 的 形式 ,为 表示 该 系 
速度 计算 公 于 内 ey 
- 一 数 引入 中 间 变量 w (一 1 一 18) ,此 时 总 动能 
四 疯 =iR. @M 十 guei 
oF -Rw Hdse, T= Pug nk ks (8) 
mB = om dts) 同样 ,分 析出 式 (6) 中 各 函数 及 变量 比 含 关系 ,如 
2 ow = 表 4 所 示 。 
Bi of =JR. (of —G201) 
图 表 4 变量 隶属 关系 表 
由 图 3 可 见 ,B; 质心 不 随 转动 而 改变 位 置 , 即 刚 a 
a 3 函数 变量 函数 变量 
体 B， 质 ， 心 速度 和 =0;B; 坐标 系 原 点 与 质心 不 重 
合 , v 久 可 通过 e 系 原点 速度 求 得 ,该 原点 速度 i'm U gag31941q 
py) =R $ py、 十 四 中 Xx 1 十 06es， 9 (3) UlrUr ~ Uo . 
其 他 刚体 质心 速度 计算 公式 如 表 3 所 示 。 .i We 
U1l0 U1l Gd2y93， 94 U17 U18 da 
表 3 刚体 质心 速度 U2 U12 U13 d3 一 96 Ua ds 
刚体 心 速度 计算 公式 214，M15 da3 91 Us 9 26 A 
PD: "ie ek Xi 入 由 表 4 可 知 ,引入 中 间 变 量 后 ,动能 工 是 w (一 
(3) 3 (1) (3) 长 和 
Vise 1R * Vige FO XC de 1 一 18) 的 函数 ,ww 又 是 gq; (i 二 2~6) 的 函数 ,根据 链 
B， vo =IR .vo XI toe 式 求 导 法 则 有 
B; vi ER .vo XI a 人 
co 机 和 Te (9) 
Be ve =3R .vo XI, 9g; 二 1 7 9g; 
由 表 2、3 可 求 出 系统 总 动能 ,系统 各 刚体 动能 分 中 间 变 量 w 中 的 g; 除 gq。 外 均 以 三 角 函 数 出现 , 故 有 
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Qu a 2 人 | 表 5 PMM 与 常规 Lagrange 方法 所 建 模型 参数 
9g; 9s; 9c; 对 比 项 目 常规 Lagrange 法 PMM 
其 中 ,一 sin gci 一 cos qi。 变量 个 数 /个 18 98 
为 避免 模型 膨胀 及 重复 运算 ,中 间 变 量 不 被 代入 黄 型 大 小 /KB 38 16 
函数 或 相关 表达 式 中 。 具 体 来 说 , 当 输 出 式 (9) 时 , 式 计算 耗 时 /us 28.8 9.7 
(10) 不 被 代入 式 (9)。 加 法 次 数 /次 2 870 1 066 
2.3 仿真 分 析 减法 次 数 /次 “44 247 
乘法 次 数 /次 7 371 2 236 
运用 Mathematic 软件 编写 相关 程序 ,并 结合 除法 次 数 /次 3 14 


行 编写 的 格式 转换 脚本 (. m 文件 ) 将 模型 自动 输出 
为 不 同 格式 (. m 或 . c) 的 文件 ,用 于 MATLAB 仿真 
或 VC 十 十 环境 下 的 控制 器 设计 。 采 用 PMM 建立 的 
斯 坦 福 机 械 臂 动力 学 模型 见 附录 。 

一 在 仿真 实验 中 ,采用 文献 [11] 中 规划 的 轨迹 和 结 
构 参 数 , 以 验证 PMM 所 建 斯 坦 福 机 械 辟 力学 模型 的 
BE 靠 性 。 设 置 仿真 时 间 为 10 s, 动 力学 第 二 类 问题 仿 
真切 析 结 果 如 图 4 所 示 。 


©O 
已 
© 02 -14 5 
TT 9 目 
Nz" 
NN" - 
-0.2 de 
© 0 5 10 0 5 10 
A Ws Us 
>> 1 -0.8 - 
站 ED 全 © 国 
> 三 05 之 -1 ra 
OG i er 
0 -2 ， 
Cs 0 5 10 0 5 10 
oe ts ts 
OO 5 40 上- 
号 三 
志 0 忆 201 
一 5 . 0 
0 5 10 0 5 10 


ts ts 


4 ， 斯坦 福 机 械 臂 仿真 曲线 


从 图 4 可 看 出 ,基于 PMM 建立 的 斯 坦 福 机 械 臂 
动力 学 模型 的 仿真 曲线 与 文献 [11] 中 仿真 曲线 一 致 ， 
验证 了 模型 的 可 靠 性 。 在 此 基础 上 ,采用 未 使 用 
PMM 的 常规 Lagrange 方法 05 建立 斯 坦 福 机 械 辟 动 
力学 模型 ,并 做 出 比较 ,如 表 5 所 示 。 

由 表 5 可 看 出 , 相 较 于 常规 Lagrange 法 ,PMIM 
建立 的 斯 坦 福 机 械 臂 动力 学 模型 在 变量 个 数 上 增加 
80 个 ,但 模型 “尺寸 "缩减 了 57.9%% ,体现 了 PMM 以 
深度 换取 宽度 的 优势 ;计算 总 操作 数 由 11 015 次 降 
为 3 563 次 ,模型 复杂 程度 降低 了 67.6%。2 种 方法 


建立 的 斯 坦 福 机 械 臂 模型 运行 在 主 频 1.8 GiB、 双 核 
CPU、2 GiB 内 存 、Win7 操作 系统 和 VC 十 十 环境 的 
工控 机 平台 上 ,进行 10 次 测试 ,PMM 所 建 模型 平均 
耗 时 9. 7 ps, 而 常规 Lagrange 方法 这 一 数值 为 28. 8 
As, 运行 效率 提高 了 66. 3%。 


3 ”PMM 在 欠 驱 动 非 完整 约束 系统 的 验证 


如 前 所 述 ,将 PMM 应 用 于 斯 坦 福 机 械 臂 完整 约 
束 系统 ,已 说 明 其 输出 动力 学 模型 在 “尺寸 "和 运行 时 
间 上 的 优势 。 为 阐明 PMM 适用 范围 和 进一步 验证 
该 方法 的 有 效 性 ,将 其 推广 到 欠 驱 动 非 完 整 约束 

变 结构 两 轮 车 机 器 人 具有 静态 不 稳定 而 动态 稳 
定 的 特性 ,是 典型 的 欠 驱 动 非 完整 约束 系统 。 以 变 结 
构 两 轮 车 机 器 人 原 地 俯仰 平衡 实验 为 例 , 机 构 简 图 及 
物理 样机 如 图 5、6 所 示 。 


(1,2,3) 
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(1,2,3) 
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图 6 


物理 样机 
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系统 由 车 架 B, 、2 个 车 又 (B,,B,) 及 2 个 车 轮 
(B;,B; ) 构 成 ,坐标 系 e*“ (一 1,2,3) 原 点 均 在 B， 
质心 处 ,ew (m 二 4,5) 原 点 均 在 B; 质心 处 ,2 个 接地 
点 分 别 为 Pl 和 了 ,。 

实验 基于 PMM 所 建立 模型 和 高 度 依 赖 系统 模 
型 的 部 分 反馈 线性 化 控制 算法 。 车 架 3 个 欧 拉 角 
qi\qz ds( 航 向 角 、 横 滚 角 、 俯 仰角 ) 由 旋转 序列 es 一 

一 es 决定 ,车 架 俯 仰角 直接 影响 车 体 的 平衡 , 取 俯 
们 和 为 扒 入 在 术 迷 业 为 物 ,于 是 

也 一 Adq3 一 qd) 十 Ru(Cdads 一 ddqsv)， (11) 
其 中 ;wv 为 虚拟 控制 量 ;gs 和 das 为 车 号 俯仰 角 和 仿 
仰角 速度 , q;。 和 dg; 为 期 望 值 ;E， 和 Av 为 控制 

rt, 一 DD .un 十 EPE， (12) 
迎 : wu 为 车 轮 电 机 转 矩 ;DD 和 EE 为 与 模型 相关 


己基 于 控制 的 控 制 参数 没 定 为 : 

〇 数值 仿真 :&, 二 一 50,k 二 1; 

二 物理 样机 实验 :， 一 100,&v 一 103。 

CVJ 图 7、8 分 别 给 出 了 仿真 结果 和 实验 数据 ,图 9 为 
物理 样机 实验 视频 连 拍 图 。 由 图 7 可 见 ,车 架 俯仰 角 
种 阐 轮 驱动 力矩 在 有 限时 间 内 快速 收 全 到 0 附近 。 
由 疼 8.9 可 见 , 车 架 俯 仰角 在 士 0.02 rad 范围 内 波 
动 秆 身 处 于 俯仰 平衡 , 故 变 结构 两 轮 车 机 器 人 实现 


了 中 地 俯仰 平衡 控制 实验 ， 


dqs/(rad's’!) 


Tt/(N-m) 


7 原 地 俯仰 平衡 仿真 曲线 


进一步 ,将 PMM 应 用 于 该 样机 摆动 车 把 模式 下 


dqy/(rad's) 


9 ” 原 地 俯仰 平衡 实验 视频 连 拍 


行走 平衡 实验 中 品 。 仅 给 出 实验 部 分 视频 连 拍 图 ， 
如 图 10 所 示 ,样机 平衡 时 间 在 10 s 以 上 。 最 终 ,应 
用 PMM 建立 的 变 结构 两 轮 车 机 器 人 力学 模型 仅 有 
11 KiB, 运行 在 DSP (TMS320F28335,150 MiB 主 
频 ) 硬 件 平 台 上 ,平均 耗 时 14. 3 ys。 由 于 物理 样机 实 
验 伺服 周期 为 33 ms, 模 型 运行 时 间 仅 占 整个 伺服 周 
期 的 0.04%。 两 者 的 时 间 差 说 明 ,PMM 模型 计算 的 
极 少量 耗 时 可 以 为 数据 采集 、 控 制 器 计算 和 数据 传输 
等 留 有 足够 的 时 间 裕 度 。 故 PMM 所 建立 的 动力 学 
模型 满足 实时 计算 的 要 求 , 验 证 了 PMM 在 变 结构 两 
轮 车 机 器 人 系统 中 的 有 效 性 与 可 靠 性 。 

将 PMM 继续 推广 到 独 轮 车 机 器 人 直线 行走 定 
位 控制 实验 "站 ,机 构 简 图 与 样机 如 图 11 所 示 。 
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(b) 物 理 样 机 


© (a) 机 构 简 图 
11 独 轮 车 机 器 人 


〇 图 12 给 出 了 实验 数据 曲线 ,图 13 为 物理 样机 实 
验 视频 连 拍 图 。 由 图 12、13 可 看 出 , 独 轮 车 机 器 人 横 
滚 角 和 俯仰 角 稳 定 在 士 0. 05 rad 范围 内 ,行走 轮 质心 
在 x 方向 上 的 位 移 从 0 m 运动 到 3 m 附近 后 ,在 小 
范围 内 保持 稳定 ,期 望 值 为 3.2 m, 误 差 仅 占 总 长 的 
6% ;行走 轮 质心 在 y 方向 上 的 位 移 在 士 0. 005 m 范 
围 内 波动 ,说 明和 车身 在 作 直线 运动 ,实现 了 独 轮 车 机 
器 人 直线 定位 控制 目标 ,验证 了 模型 可 靠 性 。 最 终 ， 
完成 独 轮 车 机 器 人 直线 定位 控制 实验 的 动力 学 模型 
仅 有 22 KiB, 运 行 在 主 频 1.8 GiB、 双 核 CPU、2 GiB 
内 存 、Win7 操作 系统 和 VC 十 十 环境 的 工控 机 平台 
上 ,平均 耗 时 24.7 ws。 独 轮 车 机 器 人 物理 样机 实验 
伺服 周期 为 44 ms, 模 型 运行 时 间 仅 占 整个 伺服 周期 
的 0.05% ,与 两 轮 车 机 器 人 情况 类 似 ,也 可 以 说 明 
PMM 输出 模型 的 计算 耗 时 可 满足 物理 样机 实验 中 
实时 计算 的 需要 ,验证 了 PMM 在 独 轮 车 机 器 人 系统 
中 的 有 效 性 。 
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13 ”直线 定位 实验 视频 连 拍 图 


综 上 所 述 ,所 提 方 法 (PMMD 满足 不 同 欠 驱动 非 
完整 约束 系统 的 基础 实验 要 求 ,验证 了 有 效 性 与 可 靠 
性 ,更 进一步 说 明 PMM 高 效 和 通用 性 强 的 特点 。 
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